#include "ProtheoII.h"
#include "./PIIProtocols/TW.h"

wxSerialPort *pDevice;

#define BLOCK_READINGS 39
typedef struct ProtheoBlock
{
	u16					FileID;//2 Qua ci metto il numero di registrazioni
	u16					BlockCounter;//
	u16					ValidReadings;//
	u16 				Readings[3][BLOCK_READINGS];//240
	u8 					Dummy[6];//6
    u16                 ECG_PERIOD[2];
	u8					ECG_STATUS[2];
	u32					Checksum;//4
}ProtheoBlock;/* 36 Bytes wide*/

ProtheoII::ProtheoII(void)
{
	Instrument::Instrument();
	/*Resolution = 1;
	DigitalOffset = 2048;
	DigitalMax = 4096;
	DigitalMin = 0;*/

	Resolution = 1;
	DigitalOffset = 0;
	//DigitalMax = 0;
	//DigitalMin = 0;
	Maximum[0]=Maximum[1]=Maximum[2] = 6; //acc in g
	Minimum[0]=Minimum[1]=Minimum[2] = -6; //acc in g
	Maximum[3] = 300;
	Minimum[3] = 0;		//HR
	
	MyName = _("ProtheoII (SXT srl)");

	Labels[0] = wxString::Format(wxT("ACC X(g)"));
	Labels[1] = wxString::Format(wxT("ACC Y(g)"));
	Labels[2] = wxString::Format(wxT("ACC Z(g)"));
	Labels[3] = wxString::Format(wxT("HR (bpm)"));
	Initialize(4,128,20);

	pDev = NULL;
	MaskAcquired	=	false;

	pDev = NULL;
}

ProtheoII::~ProtheoII(void)
{

	if(pDev != NULL)
	{
		delete pDev;
		pDev = NULL;
	}
}

#define	ECG_CHANNEL	    0
#define	BREATH_CHANNEL	1
#define	STEPS_CHANNEL_X	2
#define	STEPS_CHANNEL_Y	3
#define	STEPS_CHANNEL_Z	4

void* ProtheoII::Entry(void)
{
	float HR;
	//int indice,used;
	TW_ObjData Data;
	ProtheoBlock TempD;
	//TW_StreamingData	Data;
	
	SampleStruct	Signal;
	memset(&Signal,0,sizeof(Signal));
	
	unsigned int		ExpectedId		=	0;
	bool		SynchOk = false;

	Esci = false;
	while(!Esci)
	{
		if(IsRunning)
		{
			do{
				if(ReceiveObjData(&Data))
				{
					SynchOk = true;
				}
				else
				{
					IncErrorCounter();
					SynchOk = false;
				}
			}while(!SynchOk && !Esci);

			memcpy(&TempD,Data.Data,Data.DataLen);
			
			memset(&Signal,0,sizeof(Signal));
			Signal.Data.DataLen	=	Channels;
			for(int i = 0; i<BLOCK_READINGS;i++)
			{
				if(i == 0) 
				{
					if(TempD.ECG_PERIOD[0] != 0) HR = 60.0f/((1.0f/128.0f)*(float)(TempD.ECG_PERIOD[0]));
					else HR = 0;
				}
				if(i == (1+BLOCK_READINGS)/2)
				{
					if(TempD.ECG_PERIOD[1] != 0) HR = 60.0f/((1.0f/128.0f)*(float)(TempD.ECG_PERIOD[1]));
					else HR = 0;
				}
				
				//Signal.Data.Signal[0] =TempD.Readings[0][i];
				//Signal.Data.Signal[1] =TempD.Readings[1][i];
				//Signal.Data.Signal[2] =TempD.Readings[2][i];
				//Signal.Data.Signal[3] = 2048+HR*10;

				Signal.Data.Signal[0] =((TempD.Readings[0][i]-2048)*3.3/4096)/(3.3/15);
				Signal.Data.Signal[1] =((TempD.Readings[1][i]-2048)*3.3/4096)/(3.3/15);
				Signal.Data.Signal[2] =((TempD.Readings[2][i]-2048)*3.3/4096)/(3.3/15);
				Signal.Data.Signal[3] = HR;

				PutSample(&Signal);
			}
		}else
		{
			Sleep(100);
		}
	}

	//StopDevice();
	Sleep(1000);
	TW_Command Command;
	memset(&Command,0,sizeof(Command));
	Command.CommandID = TW_STARTSTOP;
	Command.ProtocolID = TW_COMMAND;
	Command.SenderID = 1;
	Command.Params[0]=0;

	SendCommand(&Command,0);
	pDev->Close();
	

	return NULL;
}
void ProtheoII::StartDevice(wxString FileName,wxString FilePath,bool File)
{
	TW_Command Command;
	//TW_ObjData Data;
	bool		SynchOk = false, exit = false;
	int			Dim = 0;
	//int ID;

	memset(&Command,0,sizeof(Command));
	Command.CommandID = TW_STARTSTOP;
	Command.ProtocolID = TW_COMMAND;
	Command.SenderID = 1;
	Command.Params[0]=1;
	
	SendCommand(&Command,0);
	
	
	Instrument::StartDevice(FileName,FilePath,File);
	IsRunning = true;
	/*
	if(ReceiveObjData(&Data))
	{
		
	}*/
}
void ProtheoII::StopDevice(void)
{
	
	if(IsRunning)
	{
		IsRunning	=	false;
		Esci = true;
		Instrument::StopDevice();
	}
	
	//Comando di stop
}
int ProtheoII::OpenPort(const char *PortName, const char *OpenVal2, const char *OpenVal3)
{
	wxBaud baudrate = wxBAUD_115200;

	if(pDev == NULL)	pDev = new wxSerialPort();
	if(pDev->Open(PortName) < 0)
	{
		return -1;	
	}
	pDev->SetBaudRate(baudrate);
	pDevice = pDev;
	return 1;
}